#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
机械臂控制系统 - 学生作业版本
请补全动作组9和10的代码实现

作业要求：
1. 补全动作组9：HMIRobotCatchAUVFromAssembly（从装配区夹取无人机）
2. 补全动作组10：HMIRobotPutDownAUVIntoTransferband（将无人机放置到传送带上）
3. 更新完整动作序列，包含新补全的动作组
4. 确保代码逻辑正确，包含适当的错误处理
"""

import time
import threading
import sys
import os

# 添加虚拟环境路径
venv_path = os.path.join(os.path.dirname(__file__), 'venv', 'lib', 'python3.13', 'site-packages')
if os.path.exists(venv_path):
    sys.path.insert(0, venv_path)

try:
    from canfdHand import HandClient
except ImportError as e:
    print(f"警告: 无法导入canfdHand模块: {e}")
    print("将使用模拟CAN客户端")
    # 使用模拟客户端
    class MockHandClient:
        def __init__(self):
            self.connected = True
            print("使用模拟灵巧手客户端")
        
        def grip(self):
            print("模拟执行抓取动作")
            return True
        
        def release(self):
            print("模拟执行释放动作")
            return True
    
    HandClient = MockHandClient

import Robot
from robot_controller import RobotController
from PoseDetectionClient import PoseDetectionClient
from robot_controller import run_async_in_thread


class RobotMoveControl:
    """机器人控制类，包含8个动作组，完成8个夹取和放置动作"""
    
    def __init__(self, hand_client):
        self.hand_client = hand_client
        # 定义所有动作的位置信息
        self.operation = {
            # ①②⑤⑥⑩五个动作组均为两个坐标点，pos1是位置较高的点，pos2是灵巧手抓取或释放对象的点位
            # 每个点位由6个数组成，是机械臂六个轴的关节值
            # 执行时先到达位置较高的pos1，然后移动到pos2进行抓取或放置，防止直接到达pos2发生碰撞
            "HMIRobotCatchAUVFromAGV": {
                "pos1": [-60.474, -121.317, 19.427, -159.342, 89.919, -71.347],  # 位置较高的点
                "pos2": [-59.483, -113, -6.219, -149.649, 88.998, -71.345]      # 抓取对象的点位
            },
            "HMIRobotPutDownAUVIntoCurving": {
                "pos1": [-2.359, -88.924, -54.084, -114.403, 87.353, -112.808], # 位置较高的点
                "pos2": [-14.316, -100.781, -76.108, -95.288, 88.191, -115.134] # 释放对象的点位
            },
            # ③④两个动作组设置了3个点位让机械臂带动灵巧手到达不同的位置
            # 让灵巧手的手指复位回弹到原位，能够完成后续的收缩和张开动作
            "ChildCMDRobotAutoAlignAUV": {
                "pos1": [-3.649, -118.259, -55.315, -96.346, 87.398, -112.737], # 第一个复位位置
                "pos2": [-8.808, -112.608, -55.315, -109.438, 87.716, -112.879], # 第二个复位位置
                "pos3": [-8.707, -90.419, -54.314, -110.445, 87.678, -112.878]   # 第三个复位位置
            },
            "ChildCMDRobotRecenterTray": {
                "pos1": [-71.591, -72.538, -16.107, -164.29, 90.027, -103.917],  # 第一个复位位置
                "pos2": [-68.217, -101.309, -16.258, -167.687, 83.865, -114.506], # 第二个复位位置
                "pos3": [-69.236, -106.357, -16.186, -148.067, 94.784, -129.244]  # 第三个复位位置
            },    
            "HMIRobotCatchTrayFromAGV": {
                "pos1": [-52.214, -91.189, -27.296, -137.078, 84.501, -151.648], # 位置较高的点
                "pos2": [-62.237, -104.402, -26.306, -135.265, 92.669, -162.819]  # 抓取对象的点位
            },
            "HMIRobotPutDownTrayIntoTransferband": {
                "pos1": [43.067, -88.775, -27.438, -121.666, 105.089, -89.935],  # 位置较高的点
                "pos2": [94.636, -107.845, -26.664, -135.042, 89.472, -6.325]    # 释放对象的点位
            },
            # ⑦⑨两个动作组的点位设置了视觉识别模块并抓取的动作
            # pos1仍然是位置较高的点，pos2设置为空是为了防止统一读取pos数值的时候缺失导致报错
            "HMIRobotCatchAUVFromCurving": {
                "pos1": [-2.359, -88.924, -54.084, -114.403, 87.353, -112.808], # 位置较高的点
                "pos2": []  # 空值，使用视觉识别定位
            },
            # ⑧动作组pos1是位置较高的点，pos2是灵巧手抓取对象的点位，pos3是偏移装配区一段距离的较高的点位
            # 因为机械臂需要等待装配完成再夹取无人机，防止发生碰撞
            "HMIRobotPutDownAUVIntoAssembly": {
                "pos1": [45.874, -101.848, -49.655, -108.524, 82.022, -52.312], # 位置较高的点
                "pos2": [51.118, -101.903, -50.104, -121.474, 84.132, -45.578],  # 抓取对象的点位
                "pos3": [45.978, -73.164, -61.876, -111.297, 94.548, -54.427]    # 偏移装配区的较高点位
            },
            # TODO: 学生作业 - 补全动作组9：从装配区夹取无人机
            # 坐标值已提供，学生需要补全代码逻辑
            "HMIRobotCatchAUVFromAssembly": {
                "pos1": [48.925, -100.359, -43.218, -128.538, 89.819, -48.551], # 位置较高的点
                "pos2": [],  # 空值，使用视觉识别定位
                "pos3": [49.427, -93.485, -49.944, -99.477, 77.044, -54.455]     # 完成无人机抓取后需要到达的点位
            },
            # TODO: 学生作业 - 补全动作组10：将无人机放置到传送带上
            # 坐标值已提供，学生需要补全代码逻辑
            "HMIRobotPutDownAUVIntoTransferband": {
                "pos1": [88.297, -82.864, -49.833, -110.36, 97.099, -101.297],   # 位置较高的点
                "pos2": [90.184, -93.003, -48.42, -112.981, 94.388, -97.298]     # 释放对象的点位
            },     
        }

    def action(self, key, robot): 
        """执行机器人动作组"""
        print(f"开始执行动作: {key}")
        
        # 判断是抓取还是释放动作
        if key in ['HMIRobotCatchAUVFromAGV', 'HMIRobotCatchTrayFromAGV',
                  'HMIRobotCatchAUVFromCurving', 'HMIRobotCatchAUVFromAssembly']:
            grip_action = True  # 抓取动作
        else:
            grip_action = False  # 释放动作

        if key not in self.operation:
            print(f"错误：未找到动作 {key}")
            return False
            
        positions = self.operation[key]
        position1 = positions['pos1']
        position2 = positions['pos2']
        
        # 处理特殊的三段轨迹动作（③④动作组）
        # 让灵巧手的手指复位回弹到原位，能够完成后续的收缩和张开动作
        if key == 'ChildCMDRobotAutoAlignAUV' or key == 'ChildCMDRobotRecenterTray':
            print(f"执行三段轨迹复位动作: {key}")
            print(f"移动到第一个复位位置")
            robot.MoveJ(positions['pos1'], 0, 0)
            time.sleep(0.5)
            print(f"移动到第二个复位位置")
            robot.MoveJ(positions['pos2'], 0, 0)
            time.sleep(0.5)
            print(f"移动到第三个复位位置")
            robot.MoveJ(positions['pos3'], 0, 0)
            time.sleep(0.5)
            print(f"复位动作 {key} 完成")
            return True

        # 处理需要视觉识别的动作（⑦⑨动作组）
        # pos1仍然是位置较高的点，pos2设置为空是为了防止统一读取pos数值的时候缺失导致报错
        if key == 'HMIRobotCatchAUVFromCurving':
            print("执行视觉识别抓取动作：从雕刻机抓取AUV")
            pos = [-22.969961166381836, -66.10355377197266, -82.11998748779297, 
                   -97.49507141113281, 90.33198547363281, -94.09561920166016]
            controller = RobotController(robot, pos)
            client = PoseDetectionClient()
            pose_thread = threading.Thread(target=run_async_in_thread, args=(client,), daemon=True)
            pose_thread.start()
            
            while not client.connected:
                print("等待视觉识别连接...")
                time.sleep(1)

            while True:
                while not client.ready:
                    print("等待姿态检测...")
                    time.sleep(1)
                controller.move_to_target_pose(client)
                client.ready = False
                errorCode, toolPose = robot.GetActualTCPPose(flag=1)
                print(f"当前工具姿态: {toolPose}")
                break
            time.sleep(2)
        
        # TODO: 学生作业 - 补全动作组9的视觉识别抓取逻辑
        elif key == 'HMIRobotCatchAUVFromAssembly':
            print("执行视觉识别抓取动作：从装配区抓取AUV")
            # TODO: 请参考动作组7的实现，补全以下代码：
            # 1. 设置初始位置pos（坐标值已提供）
            # 2. 创建RobotController和PoseDetectionClient
            # 3. 启动视觉识别线程
            # 4. 等待连接和姿态检测
            # 5. 执行抓取动作
            # 6. 获取当前工具姿态
            # 7. 添加适当的延时
            
            # 提示：可以参考动作组7的实现方式
            pos = [61.19493865966797, -58.597442626953125, -66.8068618774414, 
                   -120.1806411743164, 73.4983139038086, -64.21932983398438]
            # controller = RobotController(robot, pos)
            # client = PoseDetectionClient()
            # pose_thread = threading.Thread(target=run_async_in_thread, args=(client,), daemon=True)
            # pose_thread.start()
            # ... 其他代码
            
            print("TODO: 请补全动作组9的视觉识别抓取逻辑")
            time.sleep(2)
        
        else:
            # 执行常规的定点动作（①②⑤⑥⑩动作组）
            # 先到达位置较高的pos1，然后移动到pos2进行抓取或放置，防止直接到达pos2发生碰撞
            
            # TODO: 学生作业 - 补全动作组10的定点放置逻辑
            if key == 'HMIRobotPutDownAUVIntoTransferband':
                print("执行定点放置动作：将无人机放置到传送带上")
                # TODO: 请参考其他定点动作的实现，补全以下代码：
                # 1. 检查pos1和pos2是否为空（坐标值已提供）
                # 2. 移动到位置较高的点 (pos1)
                # 3. 移动到释放对象的点位 (pos2)
                # 4. 添加适当的延时
                
                print("TODO: 请补全动作组10的定点放置逻辑")
                time.sleep(2)
            else:
                # 其他已完成的动作组
                print(f"移动到位置较高的点 (pos1)")
                robot.MoveJ(position1, 0, 0)
                time.sleep(0.5)
                
                print(f"移动到抓取/释放对象的点位 (pos2)")
                robot.MoveJ(position2, 0, 0)
                time.sleep(0.5)
        
        # 执行抓取/释放动作
        if grip_action:
            print("执行抓取动作")
            self.hand_client.grip()
        else:
            print("执行释放动作")
            self.hand_client.release()
        time.sleep(0.5)
        
        # 特殊处理放置到装配区的动作（⑧动作组）
        # pos3是偏移装配区一段距离的较高的点位，因为机械臂需要等待装配完成再夹取无人机，防止发生碰撞
        if key == 'HMIRobotPutDownAUVIntoAssembly' or key == 'HMIRobotCatchAUVFromAssembly':
            position3 = positions['pos3']          
            print("移动到偏移装配区的较高点位 (pos3)")
            robot.MoveJ(position3, 0, 0)
            time.sleep(0.5)
        else:
            # 返回位置较高的点（①②⑤⑥⑩动作组）
            print("返回位置较高的点 (pos1)")
            robot.MoveJ(position1, 0, 0)
            time.sleep(0.5)
        
        # 特殊处理：放置到雕刻机后执行托盘重新居中（②动作组）
        if key == 'HMIRobotPutDownAUVIntoCurving':
            print("执行托盘重新居中动作")
            self.action('ChildCMDRobotRecenterTray', robot)
            
        print(f"动作 {key} 完成")
        return True

    def get_available_actions(self):
        """获取所有可用的动作列表"""
        return list(self.operation.keys())

    def execute_action_sequence(self, action_list, robot):
        """按顺序执行一系列动作"""
        print(f"开始执行动作序列，共 {len(action_list)} 个动作")
        for i, action in enumerate(action_list, 1):
            print(f"\n=== 执行第 {i}/{len(action_list)} 个动作: {action} ===")
            success = self.action(action, robot)
            if not success:
                print(f"动作 {action} 执行失败，停止序列")
                return False
            print(f"动作 {action} 执行成功")
        print("\n所有动作序列执行完成")
        return True


def main():
    """主函数"""
    print("=== 机械臂控制系统 - 学生作业版本 ===")
    
    # 初始化灵巧手
    print("初始化灵巧手...")
    hand_client = HandClient(channel=0)
    time.sleep(0.1)
    
    # 初始化机械臂
    print("连接机械臂...")
    robot = Robot.RPC('192.168.58.2')
    
    # 移动到初始位置
    print("移动到初始位置...")
    initpos = [-58.219, -86.098, -23.685, -121.939, 89.867, -85.634]
    robot.MoveJ(initpos, 0, 0)
    
    # 创建机器人控制对象
    robot_controller = RobotMoveControl(hand_client)
    
    # 显示可用动作
    available_actions = robot_controller.get_available_actions()
    print(f"\n可用动作列表 ({len(available_actions)} 个):")
    for i, action in enumerate(available_actions, 1):
        print(f"{i:2d}. {action}")
    
    # 交互式菜单
    while True:
        print("\n" + "="*50)
        print("请选择操作:")
        print("1. 执行单个动作")
        print("2. 执行完整动作序列")
        print("3. 显示可用动作")
        print("4. 移动到初始位置")
        print("5. 退出程序")
        
        choice = input("请输入选择 (1-5): ").strip()
        
        if choice == '1':
            print("\n可用动作:")
            for i, action in enumerate(available_actions, 1):
                print(f"{i:2d}. {action}")
            
            try:
                action_num = int(input("请输入动作编号: ")) - 1
                if 0 <= action_num < len(available_actions):
                    action_name = available_actions[action_num]
                    robot_controller.action(action_name, robot)
                else:
                    print("无效的动作编号")
            except ValueError:
                print("请输入有效的数字")
                
        elif choice == '2':
            print("\n完整动作序列:")
            sequence = [
                'HMIRobotCatchAUVFromAGV',           # ① 从AGV上夹取无人机
                'HMIRobotPutDownAUVIntoCurving',     # ② 将无人机放置到雕刻机上
                'ChildCMDRobotAutoAlignAUV',        # ③ 灵巧手在无人机处复位
                'ChildCMDRobotRecenterTray',        # ④ 灵巧手在托盘处复位
                'HMIRobotCatchTrayFromAGV',          # ⑤ 从AGV上夹取托盘
                'HMIRobotPutDownTrayIntoTransferband', # ⑥ 将托盘放置到传送带上
                'HMIRobotCatchAUVFromCurving',       # ⑦ 从雕刻机上夹取无人机
                'HMIRobotPutDownAUVIntoAssembly',    # ⑧ 将无人机放置到装配区
                'HMIRobotCatchAUVFromAssembly',      # ⑨ 从装配区夹取无人机（学生作业）
                'HMIRobotPutDownAUVIntoTransferband' # ⑩ 将无人机放置到传送带上（学生作业）
            ]
            
            confirm = input("确认执行完整动作序列? (y/n): ").strip().lower()
            if confirm == 'y':
                robot_controller.execute_action_sequence(sequence, robot)
            else:
                print("取消执行")
                
        elif choice == '3':
            print("\n可用动作列表:")
            for i, action in enumerate(available_actions, 1):
                print(f"{i:2d}. {action}")
                
        elif choice == '4':
            print("移动到初始位置...")
            robot.MoveJ(initpos, 0, 0)
            print("已移动到初始位置")
            
        elif choice == '5':
            print("退出程序")
            break
            
        else:
            print("无效选择，请重新输入")


if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        print("\n程序被用户中断")
    except Exception as e:
        print(f"程序运行出错: {e}")
    finally:
        print("程序结束")
